
import lejos.nxt.*;

public class Eyes {
	
	public static  final SensorPort ULTRASONIC_PORT  = SensorPort.S1;
	
	private static UltrasonicSensor us;
	private static int threshold;
	private static int swing;
	
	public Eyes(UltrasonicSensor sensor, int threshold, int swing){
		us = sensor;
		this.threshold = threshold;
		this.swing = swing;
	}
	
	
	public static int getFreeAngle(){
	/*
	 * Input: Threshold: how far away should the object be?
	 * 		  Swing: +-value before action should be taken.
	 * Checks for obstacle (ultrasonic). Turns ultrasonic until no object is in sight.
	 * Returns recommended rotation angle
	 * 
	 */		
		int rotation = 0;			//Ultrasonic sensor position
		boolean positive = true;	//Turn direction of usSensor

		while(obstacle()){
			
			if(rotation < 180 && positive){	//should rotate clockwise
				Motor.B.rotate(10);			//rotate 10* left
				rotation += 10;				//save rotation
			}
			else if(rotation == 180){		//finished rotated cw
				Motor.B.rotate(-190);		//rotate 190* counterclockwise
				rotation = -10;				//save rotation
				positive = false;
			}
			else if(rotation > -180 && !positive){	//should rotate ccw
				Motor.B.rotate(-10);				//rotate 10* ccw
				rotation -= 10;						//save rotation
			}
			else if(rotation == -180){			//WORK IN PROGRESS!!
				Alarm.navigationError();
				break;
			}
		}
		Motor.B.rotate(rotation*-1); 			//reset ultrasonic to straight ahead;
		return rotation;
	}	
	
	public static boolean obstacle(){
		//TROUBLESHOOTING:
		LCD.drawInt(us.getDistance(), 0, 0);
		LCD.drawInt(threshold+swing, 8, 0);
		return (us.getDistance() < threshold - swing);
	}
/*
 * 	Code-snippet is obsolete
 * 	public static boolean stairs(){
		LCD.drawInt(us.getDistance(), 0, 3);
		LCD.drawInt(threshold-swing, 8, 3);
		return threshold-swing < us.getDistance();
	}
*/
	public void avoidDanger(){
		if(obstacle()){
			Motor.A.stop();					//There is an obstacle
			Motor.C.stop();					// so.. STOP!
			int rotation = getFreeAngle();	// find free area
			
			if(rotation < 0){				
				Moves.turnRightAtSpot(rotation+100);
			}
			else{
				Moves.turnLeftAtSpot(rotation+100);
			}
		}
		
	}
	public static void main(String[] args){}


	public static boolean checkAngle(int i) {
		Motor.B.rotate(i);
		boolean ok = !obstacle();
		Motor.B.rotate(-i);

		return ok;
	}
}
